#include "includes.h"

/* amp[0]: left
 * amp[1]: right
 * amp[2]: left_middle
 * amp[3]: right_middle
 */
extern uint16 amp[4];

/* uni[0]: left
 * uni[1]: right
 * uni[2]: left_middle
 * uni[3]: right_middle
 */
extern double uni[4];

extern track_status gramb_track;
control_struct servo;

/* fuzzy input
 * f_servo_in[0]: servo.e
 */
f_io_type f_servo_in[1];
/* f_amp_in[0]: amp[0]
 * f_amp_in[1]: amp[1]
 */
f_io_type f_amp_in[2];

/* fuzzy output
 * f_servo_out[0]: servo kp
 * f_servo_out[1]: servo ki
 * f_servo_out[2]: servo kd
 */
/*
f_io_type f_servo_out[3]={{0}};
*/

/* fuzzy output debug
 * f_servo_out[0]: servo duty out
 */
f_io_type f_servo_out[1];

/* membership functions */
/* input */
/* servo error membership functions */
f_mf_type f_servo_e_memf[4];
f_mf_type f_amp_memf[2];
/* output */
/* servo duty membership functions */
f_mf_type f_servo_duty_memf[4];

/* fuzzy rule base */
f_rule_type f_servo_rule[4];
f_rule_elemtype f_servo_rule_elem[4][2];

double servo_error_calc(void){
    double unc,e[2];
    f_amp_in[0].value=amp[0];
    f_amp_in[1].value=amp[1];
    fuzzification(f_amp_in);
    unc=f_amp_memf[0].value;
    unc=MIN(unc, f_amp_memf[1].value)/100;
    unc*=unc;
    printf("unc: ");
    dtoc(unc);
    printf("\n");
    e[0]=1/uni[0]-1/uni[1];
    e[1]=1/uni[2]-1/uni[3];
    return unc*e[0]+(1-unc)*e[1];
}

void servo_fuzzy_init(void){
    int i;

    /* f_in init */
    strcpy(f_servo_in[0].name, "servo.e");
    f_servo_in[0].next=NULL;

    strcpy(f_amp_in[0].name, "amp[0]");
    strcpy(f_amp_in[1].name, "amp[1]");
    f_amp_in[0].next=f_amp_in+1;
    f_amp_in[1].next=NULL;

    /* f_out init */
    /*
    strcpy(f_servo_out[0].name, "servo kp");
    strcpy(f_servo_out[1].name, "servo ki");
    strcpy(f_servo_out[2].name, "servo kd");
    f_servo_out[0].next=f_out+1;
    f_servo_out[1].next=f_out+2;
    f_servo_out[2].next=NULL;
    */
    strcpy(f_servo_out[0].name, "duty out");
    f_servo_out[0].next=NULL;

    /* f_servo_e_memf init */
    strcpy(f_servo_e_memf[0].name, "ST");
    f_servo_e_memf[0].point1=-1;
    f_servo_e_memf[0].slope1=1000;
    f_servo_e_memf[0].point2=0.42;
    f_servo_e_memf[0].slope2=1000;

    strcpy(f_servo_e_memf[1].name, "UR");
    f_servo_e_memf[1].point1=0.2;
    f_servo_e_memf[1].slope1=125;
    f_servo_e_memf[1].point2=2;
    f_servo_e_memf[1].slope2=200;

    strcpy(f_servo_e_memf[2].name, "TR");
    f_servo_e_memf[2].point1=0.3;
    f_servo_e_memf[2].slope1=28;
    f_servo_e_memf[2].point2=10;
    f_servo_e_memf[2].slope2=50;

    strcpy(f_servo_e_memf[3].name, "LO");
    f_servo_e_memf[3].point1=0.4;
    f_servo_e_memf[3].slope1=10;
    f_servo_e_memf[3].point2=10000;
    f_servo_e_memf[3].slope2=20000;

    /* link membership functions */
    f_servo_in[0].memf=f_servo_e_memf;
    f_servo_e_memf[0].next=f_servo_e_memf+1;
    f_servo_e_memf[1].next=f_servo_e_memf+2;
    f_servo_e_memf[2].next=f_servo_e_memf+3;
    f_servo_e_memf[3].next=NULL;

    /* f_amp_memf init */
    strcpy(f_amp_memf[0].name, "LARGE");
    strcpy(f_amp_memf[1].name, "LARGE");
    f_amp_memf[0].point1=f_amp_memf[1].point1=18000;
    f_amp_memf[0].slope1=f_amp_memf[1].slope1=0.00833333;
    f_amp_memf[0].point2=f_amp_memf[1].point2=65536;
    f_amp_memf[0].slope2=f_amp_memf[1].slope2=1;

    /* link membership functions */
    f_amp_in[0].memf=f_amp_memf;
    f_amp_in[1].memf=f_amp_memf+1;
    f_amp_memf[0].next=f_amp_memf[1].next=NULL;

    /* f_servo_duty_memf init */
    strcpy(f_servo_duty_memf[0].name, "O");
    f_servo_duty_memf[0].point1=0;
    f_servo_duty_memf[0].slope1=10000;
    f_servo_duty_memf[0].point2=1;
    f_servo_duty_memf[0].slope2=10000;

    strcpy(f_servo_duty_memf[1].name, "PS");
    f_servo_duty_memf[1].point1=20;
    f_servo_duty_memf[1].slope1=10000;
    f_servo_duty_memf[1].point1=21;
    f_servo_duty_memf[1].slope2=10000;

    strcpy(f_servo_duty_memf[2].name, "PM");
    f_servo_duty_memf[2].point1=40;
    f_servo_duty_memf[2].slope1=10000;
    f_servo_duty_memf[2].point2=41;
    f_servo_duty_memf[2].slope2=10000;

    strcpy(f_servo_duty_memf[3].name, "PB");
    f_servo_duty_memf[3].point1=80;
    f_servo_duty_memf[3].slope1=10000;
    f_servo_duty_memf[3].point2=81;
    f_servo_duty_memf[3].slope2=10000;

    /* link membership functions */
    f_servo_out[0].memf=f_servo_duty_memf;
    f_servo_duty_memf[0].next=f_servo_duty_memf+1;
    f_servo_duty_memf[1].next=f_servo_duty_memf+2;
    f_servo_duty_memf[2].next=f_servo_duty_memf+3;
    f_servo_duty_memf[3].next=NULL;

    /* define servo rule base */
    for(i=0; i<4; ++i){
        f_servo_rule[i].if_side=f_servo_rule_elem[i];
        f_servo_rule[i].then_side=f_servo_rule_elem[i]+1;
        f_servo_rule[i].next=(i<3)?f_servo_rule+i+1:NULL;
        f_servo_rule_elem[i][0].next=f_servo_rule_elem[i][1].next=NULL;
        f_servo_rule_elem[i][0].value=&f_servo_e_memf[i].value;
        f_servo_rule_elem[i][1].value=&f_servo_duty_memf[i].value;
    }

    printf("servo fuzzification initialzation complete\n");
}

/* PID control */ 
/* servo_pid_k[0]: kp
 * servo_pid_k[1]: ki
 * servo_pid_k[2]: kd
 */
double servo_pid_k[3]={
    20.0,
    0.0,
    500.0
};

int servo_pid(void){
    servo.duty_out=servo_pid_k[0]*servo.e[servo.t]+servo_pid_k[1]*servo.e_int+servo_pid_k[2]*(servo.e[servo.t]-servo.e[!servo.t]);
    if(fabs(servo.duty_out)<EPS)
        return 0;
    return servo.duty_out>0?1:-1;
}

int servo_control(void){
    servo.e[servo.t]=-servo_error_calc();
    servo.e_int+=servo.e[servo.t];

    if(!gramb_track.intrack){
        switch(gramb_track.servo_turn){
            case 1:
                servo.duty_out=SERVO_DUTY_LEFT-SERVO_DUTY_MIDDLE;
                return 1;
                break;
            case -1:
                servo.duty_out=SERVO_DUTY_RIGHT-SERVO_DUTY_MIDDLE;
                return -1;
                break;
        }
    }
    
    /*
    f_servo_in[0].value=ABS(servo.e[servo.t]);
    fuzzification(f_servo_in);
    rule_evaluation(f_servo_rule);
    defuzzification(f_servo_out);
    servo.duty_out=f_servo_out[0].value*(servo.e[servo.t]>0?1:-1);
    fuzzy_refinement(f_servo_rule);
    */

    /*
    dtoc(f_servo_e_memf[0].value);
    printf("|");
    dtoc(f_servo_duty_memf[0].value);
    printf(" ");
    dtoc(f_servo_e_memf[1].value);
    printf("|");
    dtoc(f_servo_duty_memf[1].value);
    printf(" ");
    dtoc(f_servo_e_memf[2].value);
    printf("|");
    dtoc(f_servo_duty_memf[2].value);
    printf(" ");
    dtoc(f_servo_e_memf[3].value);
    printf("|");
    dtoc(f_servo_duty_memf[3].value);
    printf("\n");
    */

    return servo_pid();    
}
